/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "robotcontrol.h"
#include <QDebug>

#include "timetool.h"
#include "modbuscom.h"
#include "modbuscom.h"
#include "messagelog.h"
#include "ethercatVendorId.h"
#include <qsettings.h>
#include <QThread>
//#include "vrepcontrolnew.h"

#ifdef D_USE_IGH
#include "ecatmasterigh.h"
#endif
#ifdef D_USE_ONLY_VREP
#include "ecatmastervrep.h"
#endif
#ifdef D_USE_ACONTIS
#include "ecatmasteracontis.h"
#endif
#ifdef D_USE_CANOPEN
#include "canbuscom.h"
#endif
#include "vrepcontrol.h"
#include "vrepobject.h"
#include "encoderreceiver.h"
#include "robotbase.h"
#include "ecatlog.h"
#include "ecatbusmanager.h"
#include "ecatmasterbase.h"

#define D_SET_ZERO_SDO_TRY_COUNT 5
//VrepControl vrepControlObject;

RobotControl::RobotControl()
{

    versionInfo.componentName="EcatCore";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";

    busManager=NULL;
    ecatmaster=NULL;
    canopenCom=NULL;
    log=NULL;

    vrepCtl=NULL;

}

RobotControl::~RobotControl()
{

}

int RobotControl::getVrepSimulationRobotId(int &robotIDOut)
{
    if(3==ethercatLibType)
    {
        robotIDOut=vrepRobotId;
        return 1;
    }
    robotIDOut=-1;
    return 0;
}

int RobotControl::getEncoderReceiverOutput(unsigned short &doOut)
{
    if(1<=busManager->encoderReceiverSize)
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[0];
        return receiver->getEncoderReceiverOutput(doOut);
    }
    return -1;
}

int RobotControl::getEncoderReceiverOutput(int aliasIn, unsigned short &doOut)
{
    int tmpVectorIndex=-1;
    if(1==busManager->getDeviceArryIndexByAlias_ethercatDevice(aliasIn,tmpVectorIndex))
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[tmpVectorIndex];
        return receiver->getEncoderReceiverOutput(doOut);
    }
    return -1;
}

int RobotControl::setEncoderReceiverOutput_bit(int bitIndex, bool isHigh)
{
    if(1<=busManager->encoderReceiverSize)
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[0];
        return receiver->setEncoderReceiverOutput_bit(bitIndex,isHigh);
    }
    return -1;
}

int RobotControl::setEncoderReceiverOutput(unsigned short doIn)
{
    if(1<=busManager->encoderReceiverSize)
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[0];
        return receiver->setEncoderReceiverOutput(doIn);
    }
    return -1;
}

int RobotControl::setEncoderReceiverOutput(int aliasIn, unsigned short doIn)
{
    int tmpVectorIndex=-1;
    if(1==busManager->getDeviceArryIndexByAlias_ethercatDevice(aliasIn,tmpVectorIndex))
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[tmpVectorIndex];
        return receiver->setEncoderReceiverOutput(doIn);
    }
    return -1;
}

int RobotControl::getEncoderReceiverData_iomodule(std::vector<EncoderReceiverData> &dataListReturn)
{
    for(int m=0;m<busManager->ec_ioModuleSize;m++)
    {
        EncoderReceiverData tmpOneDeviceData;
        EcatIo* tmpIo= busManager->ec_ioModuleArry[m];
        std::vector<int> encoderValueOut;
        tmpIo->getEncoderValue(encoderValueOut);
        for(int i=0;i<encoderValueOut.size();i++)
        {
            if(0==i)
            {
                tmpOneDeviceData.Encoder0_Val=encoderValueOut[i];
            }
            else if(1==i)
            {
                tmpOneDeviceData.Encoder1_Val=encoderValueOut[i];
            }
            else if(2==i)
            {
                tmpOneDeviceData.Encoder2_Val=encoderValueOut[i];
            }

        }
        if(encoderValueOut.size()>0)
        {
            tmpOneDeviceData.deviceAlias=tmpIo->getAlias();
            dataListReturn.push_back(tmpOneDeviceData);
        }
    }
}

int RobotControl::getEncoderReceiverData_iomodule(int aliasIn, EncoderReceiverData &dataOut)
{
    //可能有问题
    int tmpVectorIndex=-1;
    if(1==busManager->getDeviceArryIndexByAlias_ethercatDevice(aliasIn,tmpVectorIndex))
    {
        EcatIo* tmpIo= busManager->ec_ioModuleArry[tmpVectorIndex];
        std::vector<int> encoderValueOut;
        tmpIo->getEncoderValue(encoderValueOut);
        for(int i=0;i<encoderValueOut.size();i++)
        {
            if(0==i)
            {
                dataOut.Encoder0_Val=encoderValueOut[i];
            }
            else if(1==i)
            {
                dataOut.Encoder1_Val=encoderValueOut[i];
            }
            else if(2==i)
            {
                dataOut.Encoder2_Val=encoderValueOut[i];
            }
            else
            {
                qDebug()<<"index exceed!";
                return -2;
            }

        }
        return 1;
    }
    qDebug()<<"no alias!"<<aliasIn;
    return -1;
}

int RobotControl::getEncoderReceiverData(EncoderReceiverData &dataOut)
{
    if(1<=busManager->encoderReceiverSize)
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[0];
        return receiver->getEncoderReceiverData(dataOut);
    }
    return -1;
}

int RobotControl::getEncoderReceiverData(int aliasIn, EncoderReceiverData &dataOut)
{
    //可能有问题
    int tmpVectorIndex=-1;
    if(1==busManager->getDeviceArryIndexByAlias_ethercatDevice(aliasIn,tmpVectorIndex))
    {
        EncoderReceiver* receiver= busManager->encoderReceiverArry[tmpVectorIndex];
        return receiver->getEncoderReceiverData(dataOut);
    }
    return -1;

}

int RobotControl::resetMasterTimeMeasure()
{
    TimeTool::getInstance()->resetAll();
    if(NULL!=canopenCom)
    {
        #ifdef D_USE_CANOPEN
        canopenCom->resetMasterTimeMeasure();
        #endif
    }
    for(int i=0;i<modbus_rtu_com.size();i++)
    {
        modbus_rtu_com[i]->resetMasterTimeMeasure();
    }
    for(int i=0;i<modbus_tcp_com.size();i++)
    {
        modbus_tcp_com[i]->resetMasterTimeMeasure();
    }
    return 1;
}

int RobotControl::resetMasterStatus()
{
    if(NULL!=canopenCom)
    {
        #ifdef D_USE_CANOPEN
        canopenCom->resetMasterStatus();
        #endif
    }
    for(int i=0;i<modbus_rtu_com.size();i++)
    {
        modbus_rtu_com[i]->resetMasterStatus();
    }
    for(int i=0;i<modbus_tcp_com.size();i++)
    {
        modbus_tcp_com[i]->resetMasterStatus();
    }
    return 1;
}

int RobotControl::writeTagData(int aliasIn, long long cardValueIn)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->writeTagData(aliasIn,cardValueIn);
    }
    else
    {
        return -1;
    }
    #endif
    return 1;
}

int RobotControl::writeTagData(int aliasIn, QVector<unsigned char> cardValueIn)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->writeTagData(aliasIn,cardValueIn);
    }
    else
    {
        return -1;
    }
#endif
return 1;
}

int RobotControl::getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
    }
    else
    {
        return -1;
    }
#endif
return 1;
}

int RobotControl::getCurrentTagData(int aliasIn, QVector<unsigned char> &cardValueOut, bool &isCardDetected)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
    }
    else
    {
        return -1;
    }
#endif
return 1;
}

int RobotControl::getMaganeticSensorRawValue(int aliasIn, QVector<bool> &sensorRawValueOut)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->getMaganeticSensorRawValue(aliasIn,sensorRawValueOut);
    }
    else
    {
        return -1;
    }
#endif
return 0;

}

int RobotControl::getBatteryInfo_fromMotor(int aliasIn, BatteryInformation &batteryInfoOut)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->getBatteryInfo_fromMotor(aliasIn,batteryInfoOut);
    }
    else
    {
        return -1;
    }
#endif
return 1;
}

int RobotControl::getBatteryInfo(int aliasIn, BatteryInformation &batteryInfoOut)
{
    #ifdef D_USE_CANOPEN
    if(NULL!=canopenCom)
    {
        return canopenCom->getBatteryInfo(aliasIn,batteryInfoOut);
    }
    else
    {
        return -1;
    }
#endif
return 1;
}

int RobotControl::controllerShutDown()
{
    #ifdef D_USE_CANOPEN
    if(NULL!=ecatmaster)
    {
        ecatmaster->controllerShutDown();
    }
    if(NULL!=canopenCom)
    {
        canopenCom->controllerShutDown();
    }
#endif
return 1;
}

int RobotControl::getEthercatDebugInfo(EthercatDebugInfo &debugInfo)
{

    if(NULL==ecatmaster)
    {
        return -1;
    }
//    return busManager->getEthercatSlavesInformation(slavesInfo);
#ifdef D_USE_IGH
    if(1==ethercatLibType && true==isUseEthercatDeivce)
    {
        return ((EcatMasterIgh*)ecatmaster)->getEthercatDebugInfo(debugInfo);
    }
#else
    return 1;
#endif
}

int RobotControl::getCanDebugInfo(EthercatDebugInfo &debugInfo)
{
    #ifdef D_USE_CANOPEN
    if(NULL==canopenCom)
    {
        return -1;
    }
    return canopenCom->getCanDebugInfo(debugInfo);
#endif
return 1;

}

int RobotControl::getModbusDebugInfo(bool isModbusRtu, int busIndex, EthercatDebugInfo &debugInfo)
{
    if(true==isModbusRtu)
    {
        if(busIndex>=0 && busIndex<modbus_rtu_com.size())
        {
            return modbus_rtu_com[busIndex]->getModbusDebugInfo(debugInfo);
        }

    }
    else if(false==isModbusRtu)
    {
        if(busIndex>=0 && busIndex<modbus_tcp_com.size())
        {
            return modbus_tcp_com[busIndex]->getModbusDebugInfo(debugInfo);
        }

    }
    return 1;
}

int RobotControl::getEthercatSlavesInformation(std::vector<EcatBusDevInfo_gsoap> &slavesInfo)
{
    if(NULL==ecatmaster)
    {
        return -1;
    }
//    return busManager->getEthercatSlavesInformation(slavesInfo);
#ifdef D_USE_IGH
    if(1==ethercatLibType && true==isUseEthercatDeivce)
    {
        return ((EcatMasterIgh*)ecatmaster)->getEthercatSlavesInformation(slavesInfo);
    }
#else
    return 1;
#endif


}

bool RobotControl::isInitialOk()
{
    if(true==isUseEthercatDeivce)
    {
        if(NULL==ecatmaster)
        {
            return false;
        }
        if(false==ecatmaster->isInitialOkStatus())
        {
            return false;
        }
    }

    if(true==isUseModbusRtuDeivce)
    {
        if(modbus_rtu_com.size()<=0)
        {
            return false;
        }
        if(NULL==modbus_rtu_com[0])
        {
            return false;
        }
        if(false==modbus_rtu_com[0]->isInitialOkStatus())
        {
            return false;
        }
    }

    if(true==isUseModbusTcpDeivce)
    {
        if(modbus_tcp_com.size()<=0)
        {
            return false;
        }
        if(NULL==modbus_tcp_com[0])
        {
            return false;
        }
        if(false==modbus_tcp_com[0]->isInitialOkStatus())
        {
            return false;
        }
    }

    if(true==isUseCanopenDevice)
    {
        #ifdef D_USE_CANOPEN
        if(NULL==canopenCom)
        {
            return false;
        }
        if(false==canopenCom->isInitialOkStatus())
        {
            return false;
        }
        #endif
    }

    return true;



}
float RobotControl::getCycle()
{
    return cycle_us;
}

int RobotControl::getRobotDi(int robID,int index, bool &diOut)
{
    RobotBase* rob;
     rob=robotsMap.value(robID, NULL);
     if( rob )
     {
         return rob->getDi(index,diOut);
     }
     return -1;

}

int RobotControl::getRobotAi(int robID, int index, double &aiOut)
{
    RobotBase* rob;
     rob=robotsMap.value(robID, NULL);
     if( rob )
     {
         return rob->getAi(index,aiOut);
     }
     return -1;
}

int RobotControl::getMaganeticDi(int robID,QVector<bool> &diOut)
{
    RobotBase* rob;

    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        return rob->getMaganeticDi(diOut);
    }
    return -1;
}

int RobotControl::setMotorControlMode(int robID,int axisIndex,E_MOTOR_CONTROL_MODE modeIn)
{
    RobotBase* rob;

    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        return rob->setMotorControlMode(axisIndex,modeIn);
    }
    return -1;
}

int RobotControl::initialMotorControlMode()
{
    RobotBase* rob=NULL;
    foreach ( rob, robotsMap )
    {
        rob->initialMotorControlMode();
    }

    return 1;
}

int RobotControl::getMotorControlMode(int robID,int axisIndex,E_MOTOR_CONTROL_MODE& modeOut)
{
    RobotBase* rob;

    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        return rob->getMotorControlMode(axisIndex,modeOut);
    }
    return -1;
}

int RobotControl::stop()
{
    if(NULL==ecatmaster)
    {
        return -1;
    }
    if(NULL!=ecatmaster)
    {
        return ecatmaster->stop();
    }
    else if(modbus_rtu_com.size()>0)
    {
        return modbus_rtu_com[0]->stop();
    }
    else if(modbus_tcp_com.size()>0)
    {
        return modbus_tcp_com[0]->stop();
    }

    return -1;
}

int RobotControl::init()
{//malloc(): memory corruption 内存访问越界导致破坏了heap的数据结构,导致随机的内存操作函数coredump
    //对错误的指针或数组指针操作了。
    //dataAnalysizer　在motorinterface 和　jointDecouple都有源码

    //初始化了robotmap和robotinfo
    QString tmpFilePath = D_TCRCONTROLLER_DATA_PATH;
    tmpFilePath+=GLOBAL_ROBOT_CONFIG;
    if(false==readRobotsXmlAndInitialRobots(tmpFilePath))
    {
        qDebug()<<"error:----------false==readRobotsXmlAndInitialRobots";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1807, -1);
        return -1;
    }

    busManager = new  EcatBusManager(this);
    QString tmpDeviceUsageFilePath = D_TCRCONTROLLER_DATA_PATH;
    tmpDeviceUsageFilePath+=GLOBAL_DEVICE_USAGE_CONFIG;
    if(1!=readDeviceUseageIniFile(tmpDeviceUsageFilePath))
    {
        return -1;
    }

    int initialResult=busManager->initial(ethercatLibType, isUseEthercatDeivce,
                                          isUseModbusRtuDeivce, isUseModbusTcpDeivce,isUseCanopenDevice);
    if(1==initialResult)
    {
        qDebug()<<"ok";

    }
    else if(-1==initialResult)
    {
        qDebug()<<"RobotControl::init() error ,readEcatBusXml(GLOBAL_ECAT_BUS_CONFIG)";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1800, -1);
        return -1;
    }
    else if(-2==initialResult)
    {
        qDebug()<<"RobotControl::init() error,readModBusRtuXml(GLOBAL_MODBUS_RTU_CONFIG)";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1801, -1);
        return -2;
    }
    else if(-3==initialResult)
    {
        qDebug()<<"RobotControl::init() error";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1803, -1);
        return -3;
    }
    else if(-4==initialResult)
    {
        qDebug()<<"RobotControl::init()readCanbusXml error";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1815, -1);
        return -3;
    }
    else
    {
        qDebug()<<"RobotControl::init() unknown error";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1816, -1);
        return -1000;
    }
//    qDebug<<"maxpriority"<<sched_get_priority_max(SCHED_FIFO);

#ifdef D_USE_IGH
    if(1==ethercatLibType && true==isUseEthercatDeivce)
    {
        ecatmaster=new EcatMasterIgh(sched_get_priority_max(SCHED_FIFO),SCHED_FIFO);
        ecatmaster->setDcCycle(busManager->dcCycle);
    }
#endif

#ifdef D_USE_ACONTIS
    if(2==ethercatLibType && true==isUseEthercatDeivce)
    {
        ecatmaster=new EcatMasterAcontis(99,SCHED_FIFO);
        ecatmaster->setDcCycle(busManager->dcCycle);

    }
#endif

#ifdef D_USE_ONLY_VREP
    if(3==ethercatLibType && true==isUseEthercatDeivce)
    {
        ecatmaster=new EcatMasterVrep();
        ecatmaster->setDcCycle(busManager->dcCycle);
    }
#endif

    cycle_us =busManager->dcCycle;
    cycle_ns =1000*cycle_us;

    //初始化robots里面的motor , io_module
    #ifdef D_USE_MODBUS_RTU
    if( true==isUseModbusRtuDeivce)
    {
        int errorOut;
        if(1!=busManager->addModbusRtuDeviceTORobotBase(errorOut))
        {
            qDebug()<<"RobotControl::init() error,busManager->addModbusRtuDeviceTORobotBase　error";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1803, -1,errorOut);
            return -4;
        }
    }
    #endif


    #ifdef D_USE_MODBUS_TCP
    if( true==isUseModbusTcpDeivce)
    {
        int error2Out;
        if(1!=busManager->addModbusTcpDeviceTORobotBase(error2Out))
        {
            qDebug()<<"RobotControl::init() error busManager->addModbusTcpDeviceTORobotBase error";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1804, -1,error2Out);
            return -5;
        }
    }
    #endif

    #ifdef D_USE_CANOPEN
    if( true==isUseCanopenDevice)
    {
        int error2Out;
        if(1!=busManager->addCanopenDeviceTORobotBase(error2Out))
        {
            qDebug()<<"RobotControl::init() error busManager->addCanopenDeviceTORobotBase error";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1818, -1,error2Out);
            return -6;
        }
    }
    #endif

    //初始化modbus
    #ifdef D_USE_MODBUS_RTU
    if( true==isUseModbusRtuDeivce)
    {
        if(busManager->aliasList_modbus_rtu.size()>0)
        {
            ModbusCom* tmpModbusCom=new ModbusCom(E_MODBUS_TYPE_RTU,0,
                      busManager->xmlFilelist_modbus_rtu,busManager->deviceTypeList_modbus_rtu,
                                                  busManager->aliasList_modbus_rtu,95,SCHED_FIFO);
            tmpModbusCom->setDcCycle(busManager->dcCycle_modbus_rtu);
            modbus_rtu_com.append(tmpModbusCom);
        }
    }
    #endif

    #ifdef D_USE_MODBUS_TCP
    if( true==isUseModbusTcpDeivce)
    {
        if(busManager->aliasList_modbus_tcp.size()>0)
        {
            tmpModbusCom=new ModbusCom(E_MODBUS_TYPE_TCP,0,
                                       busManager->xmlFilelist_modbus_tcp,busManager->deviceTypeList_modbus_tcp,
                                                                   busManager->aliasList_modbus_tcp,0,0);
            tmpModbusCom->setDcCycle(busManager->dcCycle_modbus_tcp);
            modbus_tcp_com.append(tmpModbusCom);
        }
    }
    #endif

    #ifdef D_USE_CANOPEN
    if( true==isUseCanopenDevice)
    {
        if(busManager->aliasList_canopen.size()>0)
        {
            QString canconfigFile=D_TCRCONTROLLER_DATA_PATH;
            canconfigFile+=GLOBAL_CANOPEN_PATH;
            canconfigFile+="communicateConfig.ini";
            canopenCom=new CanbusCom(canconfigFile,busManager->xmlFilelist_canopen,busManager->deviceTypeList_canopen,
                                       busManager->aliasList_canopen,busManager->busIdList_canopen,
                                     busManager->deviceIdList_canopen,98,SCHED_FIFO);
            canopenCom->setDcCycle(busManager->dcCycle_canopen);

        }
    }
    #endif
    robotsMap_onlyForCanbus=robotsMap;

    //vrep, 为什么ＩＧＨ编译时，下面的这句很容易coredump.旧版本不会，全仿真也不会。
    //全仿真模式
    if(3==ethercatLibType && true==isUseEthercatDeivce)
    {
        vrepCtl = new VrepControl(E_ECAT_VREP,E_ECAT_VREP);//有时候导致coredump有时候不会。
//        vrepCtl->setMode(E_ECAT_VREP,E_ECAT_VREP);
    }
    //(真机+仿真)模式
    else
    {
        vrepCtl = new VrepControl(E_ECAT_REAL,E_ECAT_REAL);//有时候导致coredump有时候不会。
    }

//    vrepCtl->start(100);//这个太快会堵塞
//    while(!vrepCtl->isRun)
//    {
//        QThread::usleep(100);
//    }
    RobotBase* rob=NULL;
    int i=0;
    foreach ( rob, robotsMap )
    {
        if( rob && vrepCtl->vob[i] ){
            vrepCtl->vob[i]->initVrepRobot( vrepCtl->clientID, rob->robotInfo.robotName );
            vobMap.insert( rob->robotInfo.robotId, vrepCtl->vob[i] );
            i++;
        }
         rob->setDcCycle(busManager->dcCycle);
         rob->createTimeMeasure();
    }


    //启动线程(enableSetServoData(true)只能有一个driver线程设置允许)
    #ifdef D_USE_IGH
    if(1==ethercatLibType && true==isUseEthercatDeivce)
    {
        if(true==isUseEthercatMotor && NULL!=ecatmaster)
        {
           ecatmaster->enableSetServoData(true);
        }

        ecatmaster->start();
    }
    #endif
    #ifdef D_USE_ACONTIS
    if(2==ethercatLibType && true==isUseEthercatDeivce)
    {
        if(true==isUseEthercatMotor && NULL!=ecatmaster)
        {
           ecatmaster->enableSetServoData(true);
        }
        ecatmaster->start();
    }
    #endif
    #ifdef D_USE_ONLY_VREP
    if(3==ethercatLibType && true==isUseEthercatDeivce)
    {
        if(true==isUseEthercatMotor && NULL!=ecatmaster)
        {
           ecatmaster->enableSetServoData(true);
        }
        ecatmaster->start();

    }
    #endif

    #ifdef D_USE_MODBUS_RTU
    if( true==isUseModbusRtuDeivce)
    {
        if(NULL==ecatmaster && modbus_rtu_com.size()>0)
        {
            if(true==isUseModbusRtuMotor)
            {
               modbus_rtu_com[0]->enableSetServoData(true);
            }

        }
        if(modbus_rtu_com.size()>0)
        {
            modbus_rtu_com[0]->start();
        }
    }
    #endif

    #ifdef D_USE_MODBUS_TCP
    if( true==isUseModbusTcpDeivce)
    {
        if(NULL==ecatmaster && modbus_rtu_com.size()==0 && modbus_tcp_com.size()>0)
        {
            if(true==isUseModbusTcpMotor)
            {
               modbus_tcp_com[0]->enableSetServoData(true);
            }

        }
        if(modbus_tcp_com.size()>0)
        {
            modbus_tcp_com[0]->start();
        }
    }

    #endif

    #ifdef D_USE_CANOPEN
    if( true==isUseCanopenDevice)
    {
        if(NULL==ecatmaster)
        {
            if(true==isUseCanopenMotor && NULL!=canopenCom)
            {
               canopenCom->enableSetServoData(true);
            }

        }
        if(NULL!=canopenCom)
        {
            canopenCom->start();
        }
    }

    #endif

    return 1;

}


bool RobotControl::readRobotsXmlAndInitialRobots( QString xmlfile )
{
    bool ok=openXml( xmlfile, QFile::ReadOnly | QFile::Text );
    if(!ok)
    {
        qDebug()<<"ERROR: openXml :"<<ok<< xmlfile;
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1805, -1);
        return false;
    }

    QDomNodeList nodelist = m_root.childNodes();
    int robotsCount = nodelist.size();

    if(robotsCount < 1)
    {
        qDebug()<<"ERROR: robots count<1 !";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1806, -1);
        closeXml();
        return false;
    }

    robotsMap.clear();
   // robotsFolderMap.clear();
    for(int i = 0; i < robotsCount; i++ )
    {
        QDomNode robNode = nodelist.at(i);
        if( robNode.nodeName() .indexOf("#comment")>=0){
            continue;
        }

        if( !robNode.isNull() )
        {
            int robID = robNode.toElement().attribute( "id" ).toUInt();
            QString folder = robNode.toElement().attribute( "folder" );

            RobotBase* robot=new RobotBase();
            robotsMap.insert( robID, robot );
            if(1!=robot->initRobotBase( robID, folder ))
            {
                ok=false;
                qDebug()<<" robot->init( robID, folder ), error! folder="<< folder;
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1808, robID,
                       0,0,0,0,folder);
                break;
            }

        }
        else
        {
            ok=false;
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1809,-1);
            qDebug()<<" read  xml error!"<< xmlfile;
            break;
        }
    }

    closeXml();

    return ok?true:false;
}


void RobotControl::checkError( int robID )
{
    RobotBase* rob;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob ){
                rob->checkError();
            }
        }
    }else{
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->checkError();
        }
    }
}


//===================== api =========================

//未标明(-1=所有机器人)的函数，不能传递-1参数
EN_BATTERY_STATUS RobotControl::isRobotMotorBatteryOk(int robID, int axisId)
{
    RobotBase* rob;

    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return EN_BATTERY_STATUS_AXIS_NUM_ERROR;
        }
        uint vendorErrorCode;
        int rObIndex;
        int rObSubIndex;
        LONG64 rData=-1;
        int rDataLen=4;

        int tmpVendorId=rob->logicalAxisArryCopy[axisId]->getVendorId();
        if(D_VENDOR_ID_Inovance==tmpVendorId)
        {
            rObIndex=0x203f;
            rObSubIndex=0;
            if(1==getSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
            {
                vendorErrorCode=rData;
            }
            else
            {
                qDebug()<<"isRobotMotorBatteryOk, getSdo failed!!!";
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1812,-1,axisId);
                return EN_BATTERY_STATUS_SDO_ERROR;
            }
        }
        else if(D_VENDOR_ID_YiLaiSi==tmpVendorId)
        {
            rObIndex=0x3025;
            rObSubIndex=0;
            if(1==getSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
            {
                vendorErrorCode=rData;
            }
            else
            {
                qDebug()<<"isRobotMotorBatteryOk, getSdo failed!!!";
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1812,-1,axisId);
                return EN_BATTERY_STATUS_SDO_ERROR;
            }
        }
        else if(D_VENDOR_ID_HeChuang==tmpVendorId)
        {
            rObIndex=0x213f;
            rObSubIndex=0;
            if(1==getSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
            {
                vendorErrorCode=rData;
            }
            else
            {
                qDebug()<<"isRobotMotorBatteryOk, getSdo failed!!!";
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1812,-1,axisId);
                return EN_BATTERY_STATUS_SDO_ERROR;
            }
        }
        else if(D_VENDOR_ID_VIRTUAL==tmpVendorId)
        {
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_KINCO==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_MOTEC==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_ROBOTIQ==tmpVendorId || E_VENDOR_ID_ROBOTIQ2MOTOR==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_XingSong2MOTOR==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_CURTIS2MOTOR==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(E_VENDOR_ID_AMC==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else if(D_VENDOR_ID_TECHSERVO==tmpVendorId)
        {
            //hualei need todo
            return EN_BATTERY_STATUS_NORMAL;
        }
        else
        {
            //hualei need todo

        }

        EN_BATTERY_STATUS tmpReturn=rob->isRobotMotorBatteryOk(axisId,vendorErrorCode);
        if(EN_BATTERY_STATUS_UNKNOW_VENDOR==tmpReturn)
        {
            qDebug()<<"error,,,,vendorId is not support,isRobotMotorBatteryOk,axisId="
                      <<axisId<<", vendorId="<<rob->logicalAxisArryCopy[axisId]->getVendorId();
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1811,-1,axisId,
                   rob->logicalAxisArryCopy[axisId]->getVendorId());
            return EN_BATTERY_STATUS_UNKNOW_VENDOR;
        }
        return tmpReturn;
    }
    return EN_BATTERY_STATUS_NO_ROBOT;

}


int RobotControl::clearMotorEncoderError_byNodeId(int nodeId)
{
    qDebug()<<"clearMotorEncoderError_byNodeId,nodeId="<<nodeId;//need todo

//        int rObIndex;
//        int rObSubIndex;
//        u64 rData;
//        int rDataLen;

//        if(D_VENDOR_ID_Inovance==rob->logicalAxisArryCopy[i]->getVendorId())
//        {
//            rObIndex=0x200d;
//            rObSubIndex=0x15;
//            rData=2;//1 清除报警，２　清除报警和多圈数据
//            rDataLen=2;
//        }
//        else if(D_VENDOR_ID_QingNengDeChuang==rob->logicalAxisArryCopy[i]->getVendorId())
//        {
//            rObIndex=0x6040;
//            rObSubIndex=0x0;
//            rData=0x1000;//1 清除报警，２　清除报警和多圈数据
//            rDataLen=2;
//        }
//        else
//        {
//            qDebug()<<"clearMotorBatteryError, vendor not support,axisId="
//                      <<i<<", vendorId="<<rob->logicalAxisArryCopy[i]->getVendorId();
//            return false;


//        }
//        if(1==setSdo(robID,i,rObIndex,rObSubIndex,rData,rDataLen))
//        {
//            qDebug()<<"clearMotorBatteryError ok ,axisId=="<<i;
//        }
//        else
//        {
//            qDebug()<<"clearMotorBatteryError failed ,axisId=="<<i;
//            return false;
//        }


//        return true;

    return false;
}

int RobotControl::clearMotorEncoderError_byAlias(int aliasIn)
{
 qDebug()<<"clearMotorEncoderError_byAlias,aliasIn="<<aliasIn;//need todo
}

bool RobotControl::clearMotorBatteryError(int robID)
{
    qDebug()<<"clearMotorBatteryError begin,robId="<<robID;
    RobotBase* rob;

    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        for(int i=0;i<rob->size_logicalAxisArryCopy;i++)
        {
            if(false==clearMotorBatteryErrorByAxis(robID,i))
            {
                return false;
            }
        }

        return true;
    }
    return false;
}

bool RobotControl::clearMotorBatteryErrorByAxis(int robID, int axisId)
{
    qDebug()<<"clearMotorBatteryErrorByAxis,axisId="<<axisId;

    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( NULL==rob )
    {
        return false;
    }

    int rObIndex;
    int rObSubIndex;
    u64 rData;
    int rDataLen;

    if(D_VENDOR_ID_Inovance==rob->logicalAxisArryCopy[axisId]->getVendorId())
    {
        rObIndex=0x200d;
        rObSubIndex=0x15;
        rData=2;//1 清除报警，２　清除报警和多圈数据
        rDataLen=2;
    }
    else if(D_VENDOR_ID_QingNengDeChuang==rob->logicalAxisArryCopy[axisId]->getVendorId())
    {
        rObIndex=0x6040;
        rObSubIndex=0x0;
        rData=0x1000;//1 清除报警，２　清除报警和多圈数据. bit12位控制编码器清零．
        rDataLen=2;
    }
    else if(D_VENDOR_ID_HeChuang==rob->logicalAxisArryCopy[axisId]->getVendorId())
    {
        rObIndex=0x2114;//P20.06=7,然后上电，复位多圈错误。详细错误码0x213f
        rObSubIndex=0x7;
        rData=7;//清除报警和多圈数据
        rDataLen=2;
    }
    else if(D_VENDOR_ID_Panasonic==rob->logicalAxisArryCopy[axisId]->getVendorId())
    {
        rObIndex=0x4d01;
        rObSubIndex=0x0;
        rData=0x31;//1 清除报警，２　清除报警和多圈数据
        rDataLen=2;
        setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen);

        rObIndex=0x4d00;
        rObSubIndex=0x01;
        rData=0x0;//准备
        rDataLen=4;
        setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen);

        rObIndex=0x4d00;
        rObSubIndex=0x01;
        rData=0x100;//开始
        rDataLen=4;

    }
    else if(D_VENDOR_ID_YiLaiSi==rob->logicalAxisArryCopy[axisId]->getVendorId())
    {
        rObIndex=0x2a04;
        rObSubIndex=0x0;
        rData=0x2;//1 清除报警，２　清除报警和多圈数据
        rDataLen=2;
    }
    else
    {
        qDebug()<<"clearMotorBatteryError, vendor not support,axisId="
                  <<axisId<<", vendorId="<<rob->logicalAxisArryCopy[axisId]->getVendorId();
        return false;


    }
    if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
    {
        qDebug()<<"clearMotorBatteryError ok ,axisId=="<<axisId;
    }
    else
    {
        qDebug()<<"clearMotorBatteryError failed ,axisId=="<<axisId;
        return false;
    }


    return true;
}

int RobotControl::setEthercatMotorDcUserMode(int robID)
{
    RobotBase* rob;

    qDebug()<<"setEthercatMotorDcUserMode begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        int rObIndex;
        int rObSubIndex;
        u64 rData;
        int rDataLen;
        for(int i=0;i<rob->size_logicalAxisArryCopy;i++)
        {
            if(D_VENDOR_ID_Inovance==rob->logicalAxisArryCopy[i]->getVendorId())
            {
                rObIndex=0x200c;
                rObSubIndex=0x2c;//0x2d(允许dc偏差）
                rData=0;//0 异步，１　同步１us, 2同步大于１us
                rDataLen=2;
            }
            else
            {
                //hualei need todo
                qDebug()<<"setEthercatMotorDcUserMode, vendor not support,axisId="
                          <<i<<", vendorId="<<rob->logicalAxisArryCopy[i]->getVendorId();
                return false;


            }
            if(1==setSdo(robID,i,rObIndex,rObSubIndex,rData,rDataLen))
            {
                qDebug()<<"setMotorDcMode ok ,axisId=="<<i;
            }
            else
            {
                qDebug()<<"setMotorDcMode failed ,axisId=="<<i;
                return false;
            }
        }

        return true;
    }
    return false;
}

int RobotControl::changeToHomingMode(int robID, int axisId,ZeroReturnParameter parameter)
{
    RobotBase* rob;
    qDebug()<<"RobotControl::changeToHomingMode begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);

    if( rob )
    {
        if(1!=setMotorZeroReturnParameter_multiTry(robID,axisId,parameter) )
        {
             qDebug()<<"error ,setMotorZeroReturnParameter,robId="<<robID;
             addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1810,-1,axisId);
             return -1;
        }
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return -2;
        }
        rob->logicalAxisArryCopy[axisId]->changeToHomingMode();
        return 1;
    }
    return -1;


}


int RobotControl::changeToHomingMode(int robID, QVector<ZeroReturnParameter> parameter)
{
    RobotBase* rob;
    qDebug()<<"RobotControl::changeToHomingMode begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);

    if( rob )
    {
        for(int i=0;i<rob->size_logicalAxisArryCopy;i++)
        {
            if(1!=setMotorZeroReturnParameter_multiTry(robID,i,parameter[i]) )
            {
                 qDebug()<<"error ,setMotorZeroReturnParameter,robId="<<robID;
                 addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1810,-1,i);
                 return -1;
            }
            rob->logicalAxisArryCopy[i]->changeToHomingMode();
        }
        return 1;
    }
    return -1;
}

int RobotControl::startGoZero(int robID, int axisId)
{
    RobotBase* rob;
//    qDebug()<<"RobotControl::startGoZero begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);

    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return -2;
        }
        rob->logicalAxisArryCopy[axisId]->startGoHome();
        return 1;
    }
    return -1;
}



int RobotControl::haltGoZero(int robID)
{
    RobotBase* rob;

    qDebug()<<"RobotControl::haltGoZero,robId="<<robID;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        for(int i=0;i<rob->size_logicalAxisArryCopy;i++)
        {
            rob->logicalAxisArryCopy[i]->haltGoHome();
        }
        sleep(1);
        servoOffRobot(robID);
        return 1;
    }
    return -1;
}

int RobotControl::getZeroMovingStatus(int robID, int axisId, EM_ZERO_MOVING_STATUS &statusOut)
{
    RobotBase* rob;
//    qDebug()<<"RobotControl::getZeroMovingStatus begin,robId="<<robID
//           <<" ,axisId="<<axisId;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return -2;
        }
        rob->logicalAxisArryCopy[axisId]->getZeroMovingStatus(statusOut);
        return 1;
    }
    return -1;
}

int RobotControl::restorNormalOperationMode(int robID, int axisId)
{
    RobotBase* rob;
    qDebug()<<"RobotControl::restorNormalOperationMode begin,robId="<<robID
           <<" ,axisId="<<axisId;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return -2;
        }
        rob->logicalAxisArryCopy[axisId]->restorNormalOperationMode();
        return 1;
    }
    return -1;
}

int RobotControl::restorNormalOperationMode(int robID)
{
    RobotBase* rob;
    qDebug()<<"RobotControl::restorNormalOperationMode begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        for(int i=0;i<rob->size_logicalAxisArryCopy;i++)
        {
            rob->logicalAxisArryCopy[i]->restorNormalOperationMode();
        }
        return 1;
    }
    return -1;
}

int RobotControl::setMotorTorqueLimit(int robID, int axisId, int positiveLimit, int negativeLimit)
{
    qDebug()<<"RobotControl::setMotorTorqueLimit begin,robId="<<robID
           <<" ,axisId="<<axisId;
    RobotBase* rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy || NULL==rob->logicalAxisArryCopy[axisId])
        {
            qDebug()<<"warn axisId>=size_logicalAxisArryCopy,,,RobotControl::setMotorTorqueLimit begin,robId="<<robID
                   <<" ,axisId="<<axisId;
            return 1;
        }
        if(D_VENDOR_ID_Inovance!=rob->logicalAxisArryCopy[axisId]->getVendorId())
        {
            qDebug()<<"warn: setMotorTorqueLimit ,vendor not support!";
            return 1;
        }
        int rObIndex;
        int rObSubIndex;
        u64 rData;
        int rDataLen;

        rObIndex=0x60e0;
        rObSubIndex=0x0;
        rData=positiveLimit;
        rDataLen=2;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"RobotControl::setMotorTorqueLimit+ ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"RobotControl::setMotorTorqueLimit+ ,axisId=="<<axisId;
            return -1;
        }

        rObIndex=0x60e1;
        rObSubIndex=0x0;
        rData=negativeLimit;
        rDataLen=2;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"RobotControl::setMotorTorqueLimit- ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"RobotControl::setMotorTorqueLimit- ,axisId=="<<axisId;
            return -1;
        }

    }
    return 1;
}

bool RobotControl::setMotorTorqueFilterTime(int robID, int axisId,  int filterTime)
{
    RobotBase* rob;

    qDebug()<<"RobotControl::setMotorTorqueFilterTime begin,robId="<<robID;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        int rObIndex;
        int rObSubIndex;
        u64 rData;
        int rDataLen;

        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return false;
        }

        if(D_VENDOR_ID_Inovance==rob->logicalAxisArryCopy[axisId]->getVendorId())
        {
            rObIndex=0x2007;
            rObSubIndex=0x06;
            rData=filterTime;//2007-06h,unit is 0.01ms,3000 max
            rDataLen=2;
        }
        else
        {
            //hualei need todo
            qDebug()<<"RobotControl::setMotorTorqueFilterTime, vendor not support,axisId="
                   <<axisId<<", vendorId="<<rob->logicalAxisArryCopy[axisId]->getVendorId();
            return false;


        }
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"RobotControl::setMotorTorqueFilterTime ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"RobotControl::setMotorTorqueFilterTime failed ,axisId=="<<axisId;
            return false;
        }


        return true;
    }
    return false;
}

int RobotControl::setMotorZeroReturnParameter_multiTry(int robID, int axisId, ZeroReturnParameter zeroReturnInfoIn)
{
    int tmpTryCount=D_SET_ZERO_SDO_TRY_COUNT;
    int tmpKey;
    do
    {
        tmpKey=setMotorZeroReturnParameter(robID,axisId,zeroReturnInfoIn);
        tmpTryCount--;
    }while(1!=tmpKey && tmpTryCount>0);
    return tmpKey;
}

int RobotControl::setMotorZeroReturnParameter(int robID, int axisId,
                                               ZeroReturnParameter zeroReturnInfoIn)
{

    qDebug()<<"RobotControl::setMotorZeroReturnParameter begin,robId="<<robID
           <<" ,axisId="<<axisId;
    setMotorTorqueLimit(robID,axisId,zeroReturnInfoIn.goZeroTorqueLimitPositive,
                                zeroReturnInfoIn.goZeroTorqueLimitNegative);
    RobotBase* rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        if(axisId>=rob->size_logicalAxisArryCopy ||axisId<0)
        {
            qDebug()<<"error,,,,axisId>=size_logicalAxisArryCopy ||axisId<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "RobotControl" , 1813,-1,axisId,axisId);
            return -2;
        }

        int rObIndex;
        int rObSubIndex;
        u64 rData;
        int rDataLen;

        rObIndex=0x6098;
        rObSubIndex=0x0;
        rData=zeroReturnInfoIn.goZeroMethod;
        rDataLen=1;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"goZeroMethod setSdo ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"goZeroMethod setSdo failed,axisId=="<<axisId;
            return -1;
        }

        rObIndex=0x6099;
        rObSubIndex=0x1;
        rData=zeroReturnInfoIn.goZeroSpeed;
        rDataLen=4;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"goZeroSpeed1 ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"goZeroSpeed1 setSdo failed,axisId=="<<axisId;
            return -1;
        }

        rObIndex=0x6099;
        rObSubIndex=0x2;
//        rData=zeroReturnInfoIn.goZeroSpeed/5;//回零高速和低速度差5倍
        rData=zeroReturnInfoIn.goZeroSlowSpeed;
        rDataLen=4;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"goZeroSpeed2 ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"goZeroSpeed2 setSdo failed,axisId=="<<axisId;
            return -1;
        }

        rObIndex=0x609a;
        rObSubIndex=0x0;
        rData=zeroReturnInfoIn.goZeroAcceleration;
        rDataLen=4;
        if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
        {
            qDebug()<<"goZeroAcceleration ok ,axisId=="<<axisId;
        }
        else
        {
            qDebug()<<"goZeroAcceleration setSdo failed,axisId=="<<axisId;
            return -1;
        }

        if(D_VENDOR_ID_Inovance==rob->logicalAxisArryCopy[axisId]->getVendorId())
        {
            rObIndex=0x2005;
            rObSubIndex=0x24;
            rData=zeroReturnInfoIn.goZeroTimeOutLimit*100;//unit is 10ms
            rDataLen=2;
            if(1==setSdo(robID,axisId,rObIndex,rObSubIndex,rData,rDataLen))
            {
                qDebug()<<"goZeroTimeOutLimit ok ,axisId=="<<axisId;
            }
            else
            {
                qDebug()<<"goZeroTimeOutLimit setSdo failed,axisId=="<<axisId;

            }
        }
        else
        {
            //hualei need todo
            qDebug()<<" ???warn: goZeroTimeOutLimit setSdo, vendor not support,axisId="
                      <<axisId<<", vendorId="<<rob->logicalAxisArryCopy[axisId]->getVendorId();

        }


    }
    return 1;
}

int RobotControl::readVersion(std::vector <ComponentVersion> &componentVersionList)
{
    componentVersionList.push_back(versionInfo);
    return 1;
}

bool RobotControl::servoOnRobot( int robID )
{
    qDebug()<<"RobotControl::servoOnRobot";
    if(true==isUseCanopenDevice)
    {
        #ifdef D_USE_CANOPEN
        if(NULL==canopenCom )
        {
           qDebug()<<"ERROR: NULL==canopenCom ,RobotControl::servoOnRobot";
           return false;
        }
        if(false==canopenCom->isInitialOkStatus() || true!=canopenCom->isSlavesOpStatus())
        {
            qDebug()<<"servon error,false==canopenCom->isInitialOkStatus() || true!=canopenCom->isSlavesOpStatus()";
            return false;
        }
        #endif
    }
    if(true==isUseEthercatDeivce)
    {
        if(NULL==ecatmaster )
        {
           qDebug()<<"ERROR: NULL==ecatmaster ,RobotControl::servoOnRobot";
           return false;
        }
        if(false==ecatmaster->isInitialOkStatus() || true!=ecatmaster->isSlavesOpStatus())
        {
            qDebug()<<"servon error,false==ecatmaster->isInitialOkStatus() || true!=ecatmaster->isSlavesOpStatus()";
            return false;
        }
    }

    if(true==isUseModbusRtuDeivce)
    {
        if(NULL==modbus_rtu_com[0] )
        {
           qDebug()<<"ERROR: NULL==ecatmaster ,RobotControl::servoOnRobot";
           return false;
        }
        if(false==modbus_rtu_com[0]->isInitialOkStatus() || true!=modbus_rtu_com[0]->isSlavesOpStatus())
        {
            qDebug()<<"servon error,false==ecatmaster->isInitialOkStatus() || true!=ecatmaster->isSlavesOpStatus()";
            return false;
        }
    }
    if(true==isUseModbusTcpDeivce)
    {
        if(NULL==modbus_tcp_com[0] )
        {
           qDebug()<<"ERROR: NULL==ecatmaster ,RobotControl::servoOnRobot";
           return false;
        }
        if(false==modbus_tcp_com[0]->isInitialOkStatus() || true!=modbus_tcp_com[0]->isSlavesOpStatus())
        {
            qDebug()<<"servon error,false==ecatmaster->isInitialOkStatus() || true!=ecatmaster->isSlavesOpStatus()";
            return false;
        }
    }

    RobotBase* rob;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob )
            {
                if(false==rob->servoOnRobot())
                {
                    return false;
                }
            }
        }
        return true;
    }
    else
    {
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            return rob->servoOnRobot();
        }
    }
    return false;
}

bool RobotControl::servoOnRobotAxis(int robID, int axisId)
{
    RobotBase* rob=NULL;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->servoOnRobotAxis(axisId);
    }
    return false;
}

bool RobotControl::setControlWordDebug(int robID, unsigned short controlWordIn)
{
    RobotBase* rob=NULL;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->setControlWordDebug(controlWordIn);
    }
    return false;
}

bool RobotControl::setControlWordDebug_axis(int robID, unsigned short controlWordIn, int axisId)
{
    RobotBase* rob=NULL;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->setControlWordDebug_axis(controlWordIn,axisId);
    }
    return false;
}

void RobotControl::servoSwitch( int robID , bool flag )
{
    RobotBase* rob=NULL;
    if( robID<0  )
    {
//        foreach ( rob, robotsMap )
//        {
//            if( rob ){
//                rob->servoSwitch(flag);
//            }
//        }
    }else{
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->servoSwitch(flag);
        }
    }
}

void RobotControl::servoOffRobot( int robID )
{
    RobotBase* rob=NULL;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob ){
                rob->servoOffRobot();
            }
        }
    }
    else
    {
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->servoOffRobot();
        }
    }
}

bool RobotControl::servoOffRobotAxis(int robID, int axisId)
{
    RobotBase* rob=NULL;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        rob->servoOffRobotAxis(axisId);
    }

}

void RobotControl::quickStopRobot( int robID )
{
    RobotBase* rob;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob ){
                rob->quickStopRobot();
            }
        }
    }else{
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->quickStopRobot();
        }
    }
}

int RobotControl::goHomeRobot( int robID )
{
//    RobotBase* rob;
//        rob=robotsMap.value(robID, NULL);
//        if( rob ){
//            rob->goHomeRobot();
//        }
}

//int RobotControl::setHomeOffsetRobot(int robID, QVector<int>&valueList )
//{
//    RobotBase* rob;
//    rob=robotsMap.value(robID, NULL);
//    if( rob ){
//        return rob->setHomeOffsetRobot( valueList );
//    }
//    return 0;
//}

void RobotControl::setControlWordRobot( int robID, int value )
{
    RobotBase* rob;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob ){
                rob->setControlWordRobot( value );
            }
        }
    }else{
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->setControlWordRobot( value );
        }
    }
}

QVector<ServoState> RobotControl::getServoDataRobot( int robID )
{
    if(false==robotsMap.contains(robID))
    {
        QVector<ServoState> tmpData;
        return tmpData;
    }
    RobotBase* rob;
    if( robID<0  )
    {
        //
    }
    else
    {
        rob=robotsMap.value(robID, NULL);
        if( rob )
        {
            return rob->getServoDataRobot( );
        }
    }
}

int RobotControl::resetErrorRobot( int robID )
{
//    if(eMasterErr != getMasterState())
//    {
//        return 0;
//    }

#if D_IF_DEBUG
    TimeTool::getInstance()->resetAll();
#endif
    RobotBase* rob;
    if( robID<0  )
    {
        foreach ( rob, robotsMap )
        {
            if( rob ){
                rob->resetErrorRobot( );
            }
        }
    }else{
        rob=robotsMap.value(robID, NULL);
        if( rob ){
            rob->resetErrorRobot( );
        }
    }
}

int RobotControl::clearMessage( Message msg )
{
    resetErrorRobot( msg.robotId );
}

int RobotControl::setAlias( int node , unsigned short alias )
{
    if(NULL==ecatmaster)
    {
        return -1;
    }
    int ret=1;
#if 0
    for(int i=0;i<busManager->devCount();i++)
    {
        if(busManager->ec_devInfo[i].devInfo.alias==alias )
        {
            printf("设置失败：存在重复的别名,node=%d！",i);
            ret=0;
            break;
        }
    }
#endif

    if( ret )
        ret=ecatmaster->setAlias(node, alias);
    return ret;
}



int RobotControl::robotAxisToEthercatNode(int robID, int axis, int &returnNode ,
                                          E_DEVICE_PROTOCOL &protocolOut)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        rob->robotAxisToEthercatNode( axis, returnNode ,protocolOut);
    }
    else
    {
        return false;
    }
    return 1;
}

int RobotControl::deviceAliasToEthercatNode(int deviceAliasIn, int &returnNode, E_DEVICE_PROTOCOL &protocolOut)
{

    if( NULL==canopenCom )
    {
        bool isOk=busManager->getEthercatNodeIdByAlias(deviceAliasIn,returnNode);
        protocolOut=E_DEVICE_PROTOCOL_ETHERCAT;
        return isOk;
    }
    else
    {
        returnNode=deviceAliasIn;
        protocolOut=E_DEVICE_PROTOCOL_CANOPEN;
        return true;
    }
    return 1;
}

bool RobotControl::isServoOnRobot( int robID )
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        return rob->isServoOnRobot();
    }else
    {
        return false;
    }
}

int RobotControl::getServoOnStatus(int robID,QVector<bool> &flagReturn)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
         rob->getServoOnStatus(flagReturn);
         return 1;
    }

    return -1;

}

bool RobotControl::isServoOnRobotAxis(int robID, int axisId)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob )
    {
        return rob->isServoOnRobotAxis(axisId);
    }else
    {
        return false;
    }
}

E_ECAT_MASTER_STATE RobotControl::getMasterState()
{
    if(NULL!=ecatmaster)
    {
        return ecatmaster->getMasterState();
    }
    else if(modbus_rtu_com.size()>0)
    {
        return modbus_rtu_com[0]->getMasterState();
    }
    else if(modbus_tcp_com.size()>0)
    {
        return modbus_tcp_com[0]->getMasterState();
    }
    else if(NULL!=canopenCom)
    {
        #ifdef D_USE_CANOPEN
        return canopenCom->getMasterState();
        #endif
    }

    return eMasterUnknown;
}

int RobotControl::getMotorErrorCode_by_sdo(int robID, int axisId, unsigned short &errorCodeOut)
{
    LONG64 temReturn;
    int lengthReturn=16;
    if(1==getSdo(robID,axisId,0x603f,0,temReturn,lengthReturn))
    {
        errorCodeOut=temReturn;
        return 1;
    }

    return -1;
}

int RobotControl::getMotorVendorId(int robID, int axisId, unsigned int &vendorOut)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->getMotorVendorId(axisId,vendorOut);
    }

    return -1;
}

int RobotControl::getMotorErrorCode(int robID, int axisId, unsigned short &errorCodeOut)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->getMotorErrorCode(axisId,errorCodeOut);
    }

    return -1;
}



//void RobotControl::homeParameterConfig(
//        int robID,
//        QVector <quint8> methodList,
//        QVector <int> speedToSwitch,
//        QVector <int> speedToZero,
//        QVector <int> accList )
//{

//}

int RobotControl::getMotorStatusWord(int robID,int axisId,unsigned short &statusWordOut)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->getMotorStatusWord(axisId,statusWordOut);
    }

    return -1;
}

void RobotControl::setMotorPositionRobot(int robID, QVector<int> vPos)
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        rob->setMotorPositionRobot(vPos);
    }
}

QVector<int> RobotControl::getMotorPositionRobot( int robID )
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->getMotorPositionRobot();
    }
}


QVector<int> RobotControl::getMotorPositionRobot_real( int robID )
{
    RobotBase* rob;
    rob=robotsMap.value(robID, NULL);
    if( rob ){
        return rob->getMotorPositionRobot_real();
    }
}


//void RobotControl::setAmpModeRobot( int robID,  E_MOTOR_CMD mode )
//{
//    RobotBase* rob;
//    if( robID<0 )
//    {
//        foreach ( rob, robotsMap )
//        {
//            if( rob ){
//                for(int i=0; i<rob->axis.size(); i++ )
//                {
//                    rob->axis.at(i)->modeOfOperation = mode;
//                }
//            }
//        }
//    }else{
//        rob=robotsMap.value(robID, NULL);
//        if( rob ){
//            for(int i=0; i<rob->axis.size(); i++ )
//            {
//                rob->axis.at(i)->modeOfOperation = mode;
//            }
//        }
//    }

//}


//void RobotControl::setServoDataRobot(int robID )
//{
//    RobotBase* rob;
//    if( robID<0  )
//    {
//        foreach ( rob, robotsMap )
//        {
//            if( rob ){
//                rob->setServoData( );
//            }
//        }
//    }else{
//        rob=robotsMap.value(robID, NULL);
//        if( rob ){
//            rob->setServoData( );
//        }
//    }
//}

int RobotControl::connectToVrep(QString _ip, int _port ){
    int ret=0;

    //vrep
//    if(!vrepCtl){
//        vrepCtl = new VrepControl();
//        vrepCtl->start(50);//这个太快会堵塞
//        while(!vrepCtl->isRun){
//            QThread::usleep(100);
//        }
//        RobotBase* rob=NULL;
//        int i=0;
//        foreach ( rob, robotsMap )
//        {
//            if( rob && vrepCtl->vob[i] ){
//                vrepCtl->vob[i]->initVrepRobot( vrepCtl->clientID, rob->robotInfo.robotName );
//                vobMap.insert( rob->robotInfo.robotId, vrepCtl->vob[i] );
//                i++;
//            }
//        }
//    }
    ret=vrepCtl->connectToVrep( _ip, _port );

    return ret;
}
/**
 * @brief disconnectFromVrep　断开连接
 */
void RobotControl::disconnectFromVrep( )
{

    vrepCtl->disconnectFromVrep( );

}

/**
 * @brief RobotControl::setIoSmiulationMode  io仿真模式
 * @param mode
 */
void RobotControl::setIoSmiulationMode(int mode)
{

    vrepCtl->ioSmiulationMode = mode;

}
/**
 * @brief RobotControl::setMotorSmiulationMode  motor仿真模式
 * @param mode
 */
void RobotControl::setMotorSmiulationMode(int mode)
{

    vrepCtl->motorSmiulationMode = mode;

}

int RobotControl::getSdo(  int robID,
              int axis
              ,int rObIndex
              ,int rObSubIndex
              ,LONG64 &rData
              ,int &rDataLen )
{
    LONG64 tmpData;
    int returnNode=0;
    E_DEVICE_PROTOCOL protocol;
    int ret = robotAxisToEthercatNode( robID,  axis, returnNode ,protocol);
    if( ret ==1 )
    {
        if(E_DEVICE_PROTOCOL_ETHERCAT==protocol)
        {
            if(ecatmaster )
            {
                int returnKey=ecatmaster->sdoUpload(
                            returnNode
                            , rObIndex
                            , rObSubIndex
                            , tmpData
                            , rDataLen );
                rData=tmpData;
                return returnKey;
            }
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==protocol)
        {
            #ifdef D_USE_CANOPEN
            if(canopenCom )
            {
                int returnKey=canopenCom->sdoUpload(
                            returnNode
                            , rObIndex
                            , rObSubIndex
                            , tmpData
                            , rDataLen );
                rData=tmpData;
                return returnKey;
            }
            #endif
        }

    }
    return 0;
}

int RobotControl::getSdoByNodeId(int nodeId, int rObIndex, int rObSubIndex, int64_t &rData, int &rDataLen)
{
    LONG64 tmpData;
    if(ecatmaster )
    {
        int returnKey=ecatmaster->sdoUpload(
                    nodeId
                    , rObIndex
                    , rObSubIndex
                    , tmpData
                    , rDataLen );
        rData=tmpData;
        return returnKey;
    }
    if(canopenCom )
    {
        #ifdef D_USE_CANOPEN
        int returnKey=canopenCom->sdoUpload(
                    nodeId
                    , rObIndex
                    , rObSubIndex
                    , tmpData
                    , rDataLen );
        rData=tmpData;
        return returnKey;
        #endif
    }
    return 0;
}

int RobotControl::getSdoByAlias(int alias, int rObIndex, int rObSubIndex, int64_t &rData, int &rDataLen)
{
    LONG64 tmpData;
    int returnNode=0;
    E_DEVICE_PROTOCOL protocol;
    int ret = deviceAliasToEthercatNode( alias, returnNode ,protocol);
    if( ret ==1 )
    {
        if(E_DEVICE_PROTOCOL_ETHERCAT==protocol)
        {
            if(ecatmaster )
            {
                int returnKey=ecatmaster->sdoUpload(
                            returnNode
                            , rObIndex
                            , rObSubIndex
                            , tmpData
                            , rDataLen );
                rData=tmpData;
                return returnKey;
            }
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==protocol)
        {
            #ifdef D_USE_CANOPEN
            if(canopenCom )
            {
                int returnKey=canopenCom->sdoUpload(
                            returnNode
                            , rObIndex
                            , rObSubIndex
                            , tmpData
                            , rDataLen );
                rData=tmpData;
                return returnKey;
            }
            #endif
        }

    }
    else
    {
        qDebug()<<"error,getSdoByAlias ,unknow alias"<<alias;
    }
    return 0;
}

int  RobotControl::setSdo(int robID, int axis
             , int wObIndex
             , int wObSubIndex
             , LONG64 wData
             , int wDataLen  )
{
    LONG64 tmpData;
    int returnNode=0;
    E_DEVICE_PROTOCOL protocol;
    int ret = robotAxisToEthercatNode( robID,  axis, returnNode ,protocol);
    if( ret ==1 )
    {
        if(E_DEVICE_PROTOCOL_ETHERCAT==protocol)
        {
            if( ecatmaster )
            {
                return  ecatmaster->sdoDownload(
                            returnNode
                            , wObIndex
                            , wObSubIndex
                            , (u64)wData
                            , wDataLen);
            }
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==protocol)
        {
            #ifdef D_USE_CANOPEN
            if( canopenCom )
            {
                return  canopenCom->sdoDownload(
                            returnNode
                            , wObIndex
                            , wObSubIndex
                            , (u64)wData
                            , wDataLen);
            }
            #endif
        }

    };

    return 0;

}

int RobotControl::setSdoByNodeId(int nodeId, int wObIndex, int wObSubIndex, int64_t wData, int wDataLen)
{
    if( ecatmaster )
    {
        return  ecatmaster->sdoDownload(
                    nodeId
                    , wObIndex
                    , wObSubIndex
                    , (u64)wData
                    , wDataLen);
    }
    if( canopenCom )
    {
        #ifdef D_USE_CANOPEN
        return  canopenCom->sdoDownload(
                    nodeId
                    , wObIndex
                    , wObSubIndex
                    , (u64)wData
                    , wDataLen);
        #endif
    }
    return 0;
}

int RobotControl::setSdoByAlias(int alias, int wObIndex, int wObSubIndex, int64_t wData, int wDataLen)
{
    LONG64 tmpData;
    int returnNode=0;
    E_DEVICE_PROTOCOL protocol;
    int ret = deviceAliasToEthercatNode( alias, returnNode ,protocol);
    if( ret ==1 )
    {
        if(E_DEVICE_PROTOCOL_ETHERCAT==protocol)
        {
            if( ecatmaster )
            {
                return  ecatmaster->sdoDownload(
                            returnNode
                            , wObIndex
                            , wObSubIndex
                            , (u64)wData
                            , wDataLen);
            }
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==protocol)
        {
            #ifdef D_USE_CANOPEN
            if( canopenCom )
            {
                return  canopenCom->sdoDownload(
                            returnNode
                            , wObIndex
                            , wObSubIndex
                            , (u64)wData
                            , wDataLen);
            }
            #endif
        }

    };

    return 0;
}

bool RobotControl::setDi(int robID, int index, bool valueIn)
{

}

int RobotControl::readDeviceUseageIniFile(QString filePath)
{
    ethercatLibType=0;
    isUseEthercatDeivce=false;
    isUseModbusRtuDeivce=false;
    isUseModbusTcpDeivce=false;
    isUseCanopenDevice=false;

    isUseEthercatMotor=false;
    isUseModbusRtuMotor=false;
    isUseModbusTcpMotor=false;
    isUseCanopenMotor=false;

    qDebug()<<"readDeviceUseageIniFile,fileName="<<filePath;
    QSettings settings(filePath, QSettings::IniFormat);

    QString strConfigKey;

    strConfigKey= "common/ethercatLibType";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    ethercatLibType = settings.value(strConfigKey).toInt();


    strConfigKey= "common/isUseEthercatDeivce";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseEthercatDeivce = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseModbusRtuDeivce";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseModbusRtuDeivce = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseModbusTcpDeivce";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseModbusTcpDeivce = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseCanopenDevice";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseCanopenDevice = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseEthercatMotor";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseEthercatMotor = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseModbusRtuMotor";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseModbusRtuMotor = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseModbusTcpMotor";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseModbusTcpMotor = settings.value(strConfigKey).toInt();

    strConfigKey= "common/isUseCanopenMotor";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
        return -1;
    }
    isUseCanopenMotor = settings.value(strConfigKey).toInt();

    int tmpMotorValidCount=isUseCanopenMotor+isUseModbusTcpMotor+
            isUseModbusRtuMotor+isUseEthercatMotor;
    if(1<tmpMotorValidCount)
    {
        qDebug()<<"error,,,,,deviceUserageConfig.ini motor valid count error";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1817, -1);
        return -1;
    }

    strConfigKey= "common/vrepRobotId";
    if(false==settings.contains(strConfigKey))
    {
//        qDebug()<<"error,,,,,deviceUserageConfig.ini loose parameter vrepRobotId";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "EcatMasterIgh" , 1814, -1);
//        return -1;
    }
    vrepRobotId = settings.value(strConfigKey).toInt();

    return 1;
}


void RobotControl::addMsg(int messageLevel,
                            std::string componentName,
                            std::string messageType,
                            int messageCode,
                            int robotId,
                            int parameter1 ,
                            int parameter2 ,
                            int parameter3 ,
                            int parameter4 ,
                          QString messageInfo)
{
//    struct timeval tv_begin ;
//    gettimeofday(&tv_begin, NULL);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
//    tmpMsg.time = tv_begin.tv_sec;

    QString infomationStr;
    switch(messageCode)
    {
    case 1818:
    {
        infomationStr =  QObject::tr("robots添加canopen设备失败,busDev.xml中找不到robotInfo.xml机器人配置的设备,alias=%1!").arg(parameter1);
        break;

    }
    case 1817:
    {
        infomationStr =  QObject::tr("deviceUsageConfig.ini配置不合理，只允许用一种总线的电机!");
        break;

    }
    case 1816:
    {
        infomationStr =  QObject::tr("robots添加设备失败!");
        break;

    }
    case 1815:
    {
        infomationStr =  QObject::tr("robots添加canbus设备失败,busDev.xml配置错误!");
        break;

    }
    case 1814:
    {
        infomationStr =  QObject::tr("deviceUserageConfig.ini文件缺少参数！");
        break;

    }
    case 1813:
    {
        infomationStr =  QObject::tr("机器人轴数量初始化错误! axisId=%1").arg(parameter1);
        break;

    }
    case 1812:
    {
        infomationStr =  QObject::tr("sdo失败，获取的绝对值电池信息失败!");
        break;

    }
    case 1811:
    {
        infomationStr =  QObject::tr("不支持伺服驱动品牌(axisId=%1,vendorId=%2)，获取的绝对值电池信息失败!")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1810:
    {
        infomationStr =  QObject::tr("回零参数下载到电机(%1)失败!").arg(parameter1);
        break;

    }
    case 1809:
    {
        infomationStr =  QObject::tr("robots.xml文件内容错误!");
        break;

    }
    case 1808:
    {
        infomationStr =  QObject::tr("机器人文件夹（%1)初始化失败!").arg(messageInfo);
        break;

    }
    case 1807:
    {
        infomationStr =  QObject::tr("robots.xml机器人初始化失败!");
        break;

    }
    case 1806:
    {
        infomationStr =  QObject::tr("robots.xml里面机器人数量小于１!");
        break;

    }
    case 1805:
    {
        infomationStr =  QObject::tr("找不到robots.xml配置文件错误!");
        break;

    }
    case 1804:
    {
        infomationStr =  QObject::tr("robots添加modbus_tcp设备失败,busDev.xml中找不到robotInfo.xml机器人配置的设备,alias=%1!").arg(parameter1);
        break;

    }

    case 1803:
    {
        infomationStr =  QObject::tr("robots添加modbus_rtu设备失败,busDev.xml中找不到robotInfo.xml机器人配置的设备,alias=%1!").arg(parameter1);
        break;

    }

    case 1802:
    {
        infomationStr =  QObject::tr("modbus_tcp busdev.xml初始化失败!");
        break;

    }
    case 1801:
    {
        infomationStr =  QObject::tr("modbus_rtu busdev.xml初始化失败!");
        break;

    }

    case 1800:
    {
        infomationStr =  QObject::tr("ethercat busdev.xml初始化失败!");
        break;

    }

    }
    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}


